/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This is an effort to test if the Occupancygrid can be used for SLAMing
*/ 

#include<stdio.h>
#include<stdlib.h>
#include<math.h>

#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "glutImageGUI.h"   ///< For visualisation
#include "CTopoOccuMCL.h"

#include "CScanMatchInterface.h" ///< Scanmatch interface
#include "CAHistogramECorrelationScanMatch.h" ///< Scanmatch
#include "update_pose.h" 


struct corr_st{
		float v;
		int ind;
		
		corr_st(){
				v=0;ind=0;
		}
		void set(float val, int i){
				v=val;ind=i;
		}
		bool operator<(const corr_st &s) {
				if(v<s.v) return true; 
				return false;
		}
		bool operator<=(const corr_st &s) {
				if(v<=s.v) return true; 
				return false;
		}
		bool operator>(const corr_st &s) {
				if(v>s.v) return true; 
				return false;
		}
		bool operator>=(const corr_st &s) {
				if(v>=s.v) return true; 
				return false;
		}
		
};


/// For reading the data from file

float *dist;
TOccupancyGrid *O;
int oNx,oNy;
vector<mcl::pose> godo;
vector<mcl::pose> odo;
TOccupancyGrid slamO(10.1,10.1,0.1,Grid::TCoord2D(0,0));
float *orig_map_data=NULL;


template <class T>
				unsigned int pivot (vector<T> & v, unsigned int start, 
														unsigned int stop, unsigned int position)
	// partition vector into two groups
	// values smaller than or equal to pivot
	// values larger than pivot
	// return location of pivot element
{
		// swap pivot into starting position
		swap (v[start], v[position]);

		// partition values
		unsigned int low = start + 1;
		unsigned int high = stop;
		while (low < high)
				if (v[low] < v[start])
						low++;
		else if (v[--high] < v[start])
				swap (v[low], v[high]);

		// then swap pivot back into place
		swap (v[start], v[--low]);
		return low;
}

template <class T>
				void quickSort(vector<T> & v, unsigned int low, unsigned int high)
{
	// no need to sort a vector of zero or one elements
		if (low >= high)
				return;

	// select the pivot value
		unsigned int pivotIndex = (low + high) / 2;

	// partition the vector
		pivotIndex = pivot (v, low, high, pivotIndex);

	// sort the two sub vectors
		if (low < pivotIndex)
				quickSort(v, low, pivotIndex);
		if (pivotIndex < high)
				quickSort(v, pivotIndex + 1, high);
}

template <class T> void quickSort(vector<T> & v)
{
		unsigned int numberElements = v.size ();
		if (numberElements > 1) 
				quickSort(v, 0, numberElements - 1);
}




void initMapStuff(){
		O = new TOccupancyGrid(1,1,0.04,Grid::TCoord2D(0,0));
		
		if(!O->loadLineMap("../../res/autmap-koko.txt", 1.0,1.0)){
				fprintf(stderr,"**** Unable to load the map\n");
				exit(1);
		}
		slamO.setGridToValue(0.5);
		slamO.loadLineMap("../../res/autmap-koko.txt", 1.0,1.0);
		
		O->getDim(oNx, oNy);
		dist = (float *) malloc(oNx*oNy*sizeof(float));
		orig_map_data = (float *) malloc(oNx*oNy*sizeof(float));
		float *dataPtr = O->getDataPtr();
		memcpy(orig_map_data,dataPtr,oNx*oNy*sizeof(float));
		
		
		fprintf(stderr,"*** Computing distance transform ***\n");
		O->distanceTransform(dist, 1.0);	 
		fprintf(stderr,"Done!\n");
}

float computeCorrelation(mcl::scan &meas, mcl::pose &p,float maxdist){
		float x,y,corr=0;
		Grid::TCoord2D ocoord;
		Grid::TPixel pix;
		int n=0;
		for(int i=0;i<meas.N;i++){
				if(meas.r[i]<maxdist){
						ocoord.x = p.x + meas.r[i]*cos(meas.a[i]+p.a);
						ocoord.y = p.y + meas.r[i]*sin(meas.a[i]+p.a);
						pix = O->toPixel(ocoord);
						corr+=dist[pix.y*oNx+pix.x];
						n++;
				}
		}
		if(n<40){
				corr=1000000; 
		}else corr = (corr/(float)n);
		
		
		return corr;
}

float fineTunePose(mcl::scan &meas, mcl::pose &p, float maxdist, int maxIter, float stopTh){
		ownRandom rand;
		mcl::pose tmp;
		mcl::pose best = p;
		float corr, bestcorr=10000;
		
		bestcorr = computeCorrelation(meas,p,maxdist);
		int iter=0;
		
		while(iter<maxIter && bestcorr>stopTh){
				tmp.x = p.x + rand.normalRandom()*0.2;
				tmp.y = p.y + rand.normalRandom()*0.2;
				tmp.a = p.a + (rand.normalRandom()*M_PI/180)*3.0;
				corr = computeCorrelation(meas,tmp,maxdist);
				iter++;
				
				if(corr<bestcorr){
						best = tmp;
						bestcorr = corr;
						fprintf(stderr,"Better: %.3f\n",corr);
				}
		}
		if(iter>=maxIter){
				fprintf(stderr,"Reached maximum iteration number... quitting!\n");
		}
		else{
				fprintf(stderr,"Stop threshold reached!!! \n");
		}
		p = best;
		return bestcorr;
}

float gridTunePose(mcl::scan &meas, mcl::pose &p, float maxdist, int iter){
		mcl::pose tmp;
		mcl::pose best = p;
		float corr, bestcorr=10000;
		
		bestcorr = computeCorrelation(meas,p,maxdist);
		fprintf(stderr,"First: %.3f ",bestcorr);
		
		//if(bestcorr > 0.5) return bestcorr;
		
		//int iter=0;
		
		float x,y,a;
		float POS_SEARCH = 0.08, POS_RES=0.02;
		float ANG_SEARCH = 2.0*M_PI/180.0;
		float ANG_RES = M_PI/720.0;
		while(iter > 0){
				for(x=p.x-POS_SEARCH*(float)iter;x<p.x+POS_SEARCH*(float)iter;x+=POS_RES*(float)iter){
						for(y=p.y-POS_SEARCH*(float)iter;y<p.y+POS_SEARCH*(float)iter;y+=POS_RES*(float)iter){
								for(a=p.a-ANG_SEARCH*(float)iter;a<p.a+ANG_SEARCH*(float)iter;a+=ANG_RES*(float)iter){
										tmp.set(x,y,a);
										corr = computeCorrelation(meas,tmp,maxdist);
										if(corr<bestcorr){
												best = tmp;
												bestcorr = corr;
												//fprintf(stderr,"Better: %.3f\n",corr);
										}
								}///A
						}///Y
				}///X
				fprintf(stderr,"Best: %.3f\n",bestcorr);
				p = best;
				iter--;
		}
		return bestcorr;
}

float gridTunePose2(mcl::scan &meas, mcl::pose &p, float maxdist, int iter){
		mcl::pose tmp;
		mcl::pose best = p;
		float corr, bestcorr=10000;
		
		bestcorr = computeCorrelation(meas,p,maxdist);
		fprintf(stderr,"First: %.3f ",bestcorr);
		
		//if(bestcorr > 0.5) return bestcorr;
		
		//int iter=0;
		
		float x,y,a;
		float POS_SEARCH = 0.08, POS_RES=0.02;
		float ANG_SEARCH = 2.0*M_PI/180.0;
		float ANG_RES = M_PI/720.0;
		
		for(int i=0;i<iter;i++){
				POS_SEARCH*=2.0;
				POS_RES *= 2.0;
				ANG_SEARCH *= 2.0;
				ANG_RES*=2.0;
		}
		
		while(iter > 0){
				for(x=p.x-POS_SEARCH;x<p.x+POS_SEARCH;x+=POS_RES){
						for(y=p.y-POS_SEARCH;y<p.y+POS_SEARCH;y+=POS_RES){
								for(a=p.a-ANG_SEARCH;a<p.a+ANG_SEARCH;a+=ANG_RES){
										tmp.set(x,y,a);
										corr = computeCorrelation(meas,tmp,maxdist);
										if(corr<bestcorr){
												best = tmp;
												bestcorr = corr;
												//fprintf(stderr,"Better: %.3f\n",corr);
										}
								}///A
						}///Y
				}///X
				p = best;
				iter--;
				POS_SEARCH/=2.0;
				POS_RES /= 2.0;
				ANG_SEARCH /= 2.0;
				ANG_RES/=2.0;
		}
		fprintf(stderr,"Best: %.3f\n",bestcorr);
		return bestcorr;
}


static int lastCorrected=-1;
void odoCorrector(mcl::pose &p, int index){
		mcl::pose mest;
		mcl::pose corPos;
		
		if(lastCorrected == -1){
				lastCorrected = index;
				godo[index]=p;
				for(int i=index;i<godo.size()-1;i++){
						mest.setToDifferentialPose( odo[i+1],odo[i]);
						corPos = godo[i].integrateDifferential(mest);
						godo[i+1] = corPos;
				}
				for(int i=index;i>0;i--){
						mest.setToDifferentialPose( odo[i-1],odo[i]);
						corPos = godo[i].integrateDifferential(mest);
						godo[i-1] = corPos;
				}	
		}
		else{
				mcl::pose orig = godo[index];
				mcl::pose err = orig;
				int N = index-lastCorrected;
				int last = lastCorrected;
						
				err.x -= p.x; 
				err.y -= p.y; 
				err.a -= p.a; 
				fprintf(stderr,"Orig_err (%.2f %.2f %.2f) ", err.x,err.y,err.a);
				err.a/=(float)(N);
				
				
				lastCorrected = index;
				godo[index]=p;
				for(int i=index;i<godo.size()-1;i++){
						mest.setToDifferentialPose( odo[i+1],odo[i]);
						corPos = godo[i].integrateDifferential(mest);
						godo[i+1] = corPos;
				}
				///Now compute the correction from the last correctio to present
				/// First the heading
				godo[index] = orig;
				for(int i=last;i<index;i++){
					  mest.setToDifferentialPose( godo[i+1],godo[i]);
						if(i>last) godo[i] = corPos; 
						mest.a -= err.a;
						corPos = godo[i].integrateDifferential(mest);
				}
				godo[index]=corPos;
			//	mest.setToDifferentialPose( godo[i+1],godo[i]);
			//	mest.a += err.a;
			//	corPos = godo[i].integrateDifferential(mest);
				
				
				
				///////// Virhe ....
				err = godo[index];
			 	err.x -= p.x; 
			 	err.y -= p.y; 
			 	err.a -= p.a; 
			 	fprintf(stderr,"Err_Now (%.2f %.2f %.2f) ", err.x,err.y,err.a);
				err.y/=(float)(N);
				err.x/=(float)(N);
				
				for(int i=last;i<=index;i++){
					godo[i].x -= (i-last)*err.x;
					godo[i].y -= (i-last)*err.y;
				}
				err = godo[index];
				err.x -= p.x; 
				err.y -= p.y; 
				err.a -= p.a; 
				fprintf(stderr,"Final (%.2f %.2f %.2f) ", err.x,err.y,err.a);
				
		}
}



int readScanLine181(FILE *f,mcl::scan &meas, mcl::pose &p){
		float time_s;
		int i;
		char c;
		fscanf(f,"%f %f %f",&p.x,&p.y,&p.a); ///< Pose
	//fprintf(stderr,"1 ");
		for(i=0;i<181;i++){
				fscanf(f,"%f %f",&meas.r[i],&meas.a[i]);
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<181){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
	//fprintf(stderr,"3 \n");
		meas.N = 181;
		return 0;
}

int readSilmuPoseData(FILE *f,mcl::scan &meas, mcl::pose &p){
		float t1,t2;
		int i;
		char c;
		fscanf(f,"%f %f %f %f %f %d\n",&t1,&p.x,&p.y,&t2,&p.a,&i); ///< Pose
		meas.N = 0;
		
		return 0;
}

 int readSilmuPoseDataNuppu(FILE *f,mcl::scan &meas, mcl::pose &p){
		float t1,t2,foo1,foo2;
		int i;
		char c;
		//fscanf(f,"%f %f %f %f %f %d %f %f\n",&t1,&p.x,&p.y,&t2,&p.a,&i,&foo1,&foo2); ///< Pose
		fscanf(f,"%f %f %f %f %f %d %f %f\n",&t1,&foo1,&foo2,&t2,&p.a,&i,&p.x,&p.y); ///< Pose
		meas.N = 0;
		
		return 0;
}


int readLaserOdometryData(FILE *f,mcl::scan &meas, mcl::pose &p){
		float p1,p2,p3;
		int i;
		char c;
		fscanf(f,"%f %f %f %f %f %f ", &p.x,&p.y,&p.a,&p1,&p2,&p3); ///< Pose
		for(i=0;i<361;i++){
				fscanf(f,"%f",&meas.r[i]);
				meas.a[i]=0.5*i*M_PI/180.0;
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<361){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
		meas.N = 361;
		return 0;

}



void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
	rgb[3*Nx*j+3*i]=r;
	rgb[3*Nx*j+3*i+1]=g;
	rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
	float *data = gr->getDataPtr();
	int Nx,Ny;
	gr->getDim(Nx,Ny);
	float v;
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
				v = data[Nx*j+i];
				//if(v>0.29) v = 0.9;
				setRGBpix(rgb,i,j, Nx, r*v,g*v,b*v);
		}
	}	
}

void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
		for(int ii=i-1;ii<i+1;ii++){
				for(int jj=j-1;jj<j+1;jj++){
						setRGBpix(rgb,ii,jj,Nx,r,g,b);
				}
		}

}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
		float dy = sin(angle);
		float dx = cos(angle);
		float rr=0;
		int i=0;
		uint x,y;
		while(rr<length){
				x = pix + i*dx;
				y = piy - i*dy;
				RGB[3*Nx*y+3*x] = r;
				RGB[3*Nx*y+3*x+1] = g;
				RGB[3*Nx*y+3*x+2] = b;
				i++;
				rr+=resolution;
		}
}

int main(void){
		float grid_size = 45.0;
		
		CTopoOccuMCL topo;
		grid_t *origmap;
		grid_t *dataPtr;
		int Nx,Ny;
		unsigned char *rgb,*rgb2;
		//FILE *log = fopen("j2b2_measurement.txt","rt");
		//FILE *log = fopen("silmupose_data.txt","rt");
		FILE *log = fopen("laser_update.txt","rt");
		if(log==NULL){
			fprintf(stderr,"No such file\n");	
			return 0;
		}
		FILE *cor_odo=fopen("topo_odo.txt","rt");
		if(cor_odo==NULL){
				fprintf(stderr,"No Odometry file\n");	
				return 0;
		}
		FILE *ftopo=fopen("topo_path.txt","rt");
		if(ftopo==NULL){
			fprintf(stderr,"No Odometry file\n");	
			return 0;
		}
		
		
		initMapStuff(); ///< For correlation
		
		///For SLAM matching
		ScanMatch::scan meas,ref;  ///< The Reference scan 
		meas.allocate(361);
		ref.allocate(361);
		Grid::TScan virt; ///< for occupancy grid
		virt.alloc(361);
		ScanMatch::pose smpos;
		CScanMatchInterface *gmatcher; ///< This is for global
	
		gmatcher = new CAHistogramECorrelationScanMatch(
																				.64,   // Search area in meters 
																				0.6);     // Search angle in radians
		
	/*
		topo.initialize("../../res/autmap-new.txt",
										1.0, 
										10000, 
										mcl::pose(-22,1,70*M_PI/180),  
										mcl::pose(0.1,0.1,M_PI/30),
										0.1,
										1.0);
		*/
		
		
		/**
		* Robo room!
		topo.initializeGlobal("../../res/autmap-new.txt",
													1.0, 
													0.1,  
			 										mcl::pose(0.4,0.4,M_PI/10), 
													mcl::pose(8,-8,0),	mcl::pose(15,0,2*M_PI));
		****/
		
		topo.initializeGlobal("../../res/autmap-koko.txt",
													1.0, 
													0.1,  
													mcl::pose(0.4,0.4,M_PI/20), 
													mcl::pose(-22,-2,0),	mcl::pose(-10,2,2*M_PI));
		
		topo.O->getDim(Nx,Ny);
		fprintf(stderr,"Map size = %dx%d\n",Nx,Ny);
		origmap = (grid_t *) malloc(Nx*Ny*sizeof(float));
		dataPtr = topo.O->getDataPtr();
		memcpy(origmap,dataPtr,Nx*Ny*sizeof(float));		
		
		///Initialize Image display
		int slamNx,slamNy;
		slamO.getDim(slamNx,slamNy);
		fprintf(stderr,"Image size = (%d,%d)\n",slamNx,slamNy);
		//rgb = (unsigned char *) malloc(3*Nx*Ny);
		rgb2 = (unsigned char *) malloc(3*slamNx*slamNy);
		
		//addImageWindow(Nx, Ny);
		addImageWindow(slamNx, slamNy);
		glutImageGUI_Start();
		
		mcl::scan s; ///< for occupancy grid
		s.allocate(361);
		mcl::pose odo_cur, odo_ref,mest;
		Grid::TScan gscan;
		gscan.alloc(361);
		//readScanLine181(log,s,odo_cur);
		//readSilmuPoseData(log,s,odo_cur);
		//readSilmuPoseDataNuppu(log,s,odo_cur);
		//readLaserOdometryData(log,s,odo_ref);
		
		
		displayImage(0, rgb2,slamNx,slamNy);
		
		long int ss0,us0,ss1,us1,ss2,us2,sec,us,ms;
		float fms;
		ownTime_GetTimeOfDay(&ss0,&us0);
		int cnt=0;
		
		vector<mcl::pose> orig_odo;
		vector<mcl::pose> path;
		vector<mcl::pose> path_final;
		vector<mcl::pose> path_back;
		vector<float> path_var;
		vector<float> path_back_var;
		vector<mcl::scan> scans;
		
		vector<mcl::pose> topo_variance;
		vector<mcl::pose> topo_pose;
		mcl::pose tmp_pos;
		
		while(!feof(log)){
				readLaserOdometryData(log,s,odo_cur);
				orig_odo.push_back(odo_cur);
				//odo.push_back(odo_cur);
				//godo.push_back(odo_cur);
				scans.push_back(s);
				fscanf(cor_odo,"%f %f %f\n",&odo_cur.x,&odo_cur.y,&odo_cur.a); ///< Pose
				odo.push_back(odo_cur);
				float tmp1,tmp2,tmp3;
				fscanf(ftopo,"%f %f %f %f %f %f\n",&tmp_pos.x,&tmp_pos.y,&tmp_pos.a,&odo_cur.x,&odo_cur.y,&odo_cur.a); ///< Pose
				topo_pose.push_back(tmp_pos);
				topo_variance.push_back(odo_cur);
		}
		path_final = odo;
		
		
		vector<corr_st> correlations;
		corr_st tmp_cor;
		/**
		 * Compute correlation for each topological pose 
		 **/
		fprintf(stderr,"Computing the correlation for whole data set\n");
		for(int i=0;i<odo.size();i+=1){
				tmp_cor.v = computeCorrelation(scans[i], odo[i],8.0);
				tmp_cor.ind = i;
				correlations.push_back(tmp_cor);
				if(i%250==0) fprintf(stderr,"%.2f completed\n",100.0*(float)(i)/(float)odo.size()); 
		}
		quickSort(correlations);
				
		cnt=0;
		int draw = 0;
		Grid::TCoord2D ocoord;
		Grid::TCoord2D coord;
		Grid::TPixel pix;
		int cor_cnt=0;
		int end=0;
		
		
		////////////////////////////////////
		vector<mcl::pose> pose_meas;
		vector<mcl::pose> pose_meas_var;
		vector<int> pose_meas_ind;
		float cor_threshold = 1115;
		float cor_th_step=2.0;
		bool reset = false; 
		////////////////////////////////////
		UpdatePose pose_updater;
		pose_updater.initialize(orig_odo,4);
		/******************************************************************************
		pose_meas = topo_pose; ///< initial guess
		pose_meas_var = topo_variance;
		fprintf(stderr,"SIZE %d should be %d and also %d\n",pose_meas_var.size(),odo.size(),orig_odo.size());
		for(int i=0;i<pose_meas.size();i++){
			//pose_meas_var.push_back(mcl::pose(1,1,1));
			pose_meas_ind.push_back(i);
		}
		pose_updater.update(pose_meas, pose_meas_ind, pose_meas_var);
		for(int i=0; i<6;i++){
			fprintf(stderr,"Updating the odometry - iteration %d!\n",i);
			setRGBmap(rgb2,&slamO, 200,255, 200);
			for(int j=0;j<odo.size();j++){					
				ocoord.x = pose_updater.Xpos[3*j];ocoord.y =pose_updater.Xpos[3*j+1];
				pix = slamO.toPixel(ocoord);
				setRGBpixBold(rgb2,pix.x,pix.y, slamNx, 255,255, 0 );	
				
				ocoord.x = topo_pose[j].x;ocoord.y = topo_pose[j].y;
				pix = slamO.toPixel(ocoord);
				setRGBpixBold(rgb2,pix.x,pix.y, slamNx, 255,0, 0 );
					
				ocoord.x = orig_odo[j].x;ocoord.y = orig_odo[j].y;
				pix = slamO.toPixel(ocoord);
				setRGBpixBold(rgb2,pix.x,pix.y, slamNx, 255,255, 255 );	
			}
			displayImage(0, rgb2,slamNy,slamNx);
			usleep(1000*1000);
			pose_updater.iterate();
		}
		
		pose_meas.clear();pose_meas_var.clear();pose_meas_ind.clear();
		for(int i=0;i<odo.size();i++){	
				odo[i].set(pose_updater.Xpos[3*i],pose_updater.Xpos[3*i+1],pose_updater.Xpos[3*i+2]);
		}
		pose_meas = odo; ///< initial guess
		for(int i=0;i<pose_meas.size();i++){
				pose_meas_var.push_back(mcl::pose(1000000,1000000,1000000));
				pose_meas_ind.push_back(i);
		}
		path_final = odo;
		usleep(5000*1000);
		*/
				
		while(1){
			
			
			while(!reset){
				if((cnt+1)%50==0){ 
						draw = 1;
						fprintf(stderr,"Updating the MAP distance transform\n");
						O->distanceTransform(dist, 0.8);
				}
				
				///Processing completed - time to fit the odometry to the map
				if(cor_cnt<scans.size()){
						int cur_ind = correlations[cor_cnt].ind;  ///< This is the next index	
											
						mcl::pose tune =  odo[cur_ind];
						
						O->getScan(tune.x,tune.y, (tune.a), 361, M_PI/360.0, (grid_t) 0.7, virt); ///<getscan
						for(int i=0;i<meas.N;i++){
							ref.a[i]=virt.a[i];ref.r[i]=virt.r[i];
							meas.a[i]=scans[cur_ind].a[i]; meas.r[i]=scans[cur_ind].r[i];
						}
						/*
						smpos.set(tune.x,tune.y,tune.a);
						gmatcher->setCurrentPose(smpos);
						gmatcher->setReferenceScan(ref,smpos); 
						gmatcher->update(meas,smpos);    ///update
						gmatcher->getCurrentPose(smpos); ///< Get the estimate						
						tune.set(smpos.x,smpos.y,smpos.a);
						*/
						float corr_val=gridTunePose2(scans[cur_ind],tune , 8.0,2);
	
						for(int j=0;j<scans[cur_ind].N;j++){
								gscan.r[j] = scans[cur_ind].r[j];
								if(gscan.r[j] > 8.0) gscan.r[j] = 0; 
								gscan.a[j] = scans[cur_ind].a[j];
						}
						slamO.setScanFillProb(tune.x, tune.y ,tune.a, gscan,0.3,0.7);
						O->setScanFillProb(tune.x, tune.y ,tune.a, gscan,0.3,0.95);
						path_final[cur_ind] = tune;
						
						
						pose_meas.push_back(tune);
						pose_meas_var.push_back(mcl::pose(corr_val,corr_val,corr_val));
						pose_meas_ind.push_back(cur_ind);
						/******
						pose_meas[cur_ind]=tune;
						pose_meas_var[cur_ind]=mcl::pose(corr_val,corr_val,corr_val);
					  *****/
						if(corr_val>cor_threshold) reset = true;
					}else{
							if(end==0){
									//////////////////////////////////////////////////////////////////////////////
									//////////////////////////////////////////////////////////////////////////////
								//////////////////////////////////////////////////////////////////////////////
									fprintf(stderr,"Resetting!!!\n");
									reset = false;
									cnt = 0; cor_cnt = 0;
									float *map_data_ptr = O->getDataPtr();
									memcpy(map_data_ptr,orig_map_data,sizeof(float)*oNx*oNy);
									pose_updater.update(pose_meas, pose_meas_ind, pose_meas_var);
									for(int i=0; i<6;i++){
											fprintf(stderr,"Updating the odometry - iteration %d!\n",i);
											setRGBmap(rgb2,&slamO, 200,255, 200);
				
											mcl::pose odo_pose_calc(-20,-25,0);
											mcl::pose diffi;
											for(int j=0;j<odo.size()-1;j++){					
													ocoord.x = pose_updater.Xpos[3*j];ocoord.y =pose_updater.Xpos[3*j+1];
													pix = slamO.toPixel(ocoord);
													setRGBpixBold(rgb2,pix.x,pix.y, slamNx, 0,255, 0 );	
					
													diffi.set(pose_updater.odo[3*j],pose_updater.odo[3*j+1],pose_updater.odo[3*j+2]);
													odo_pose_calc = odo_pose_calc.integrateDifferential(diffi);
													ocoord.x = odo_pose_calc.x;ocoord.y =odo_pose_calc.y;
													pix = slamO.toPixel(ocoord);
													setRGBpixBold(rgb2,pix.x,pix.y, slamNx, 100,100, 100 );				
											}
				
				
				
											displayImage(0, rgb2,slamNy,slamNx);
											usleep(1000*1000);
											pose_updater.iterate();
									}
									pose_meas.clear();pose_meas_var.clear();pose_meas_ind.clear();
			
									for(int i=0;i<odo.size();i++){			
											odo[i].set(pose_updater.Xpos[3*i],pose_updater.Xpos[3*i+1],pose_updater.Xpos[3*i+2]);
									}
									path_final = odo;
									//////////////////////////////////////////////////////////////////////////////
									//////////////////////////////////////////////////////////////////////////////
									//////////////////////////////////////////////////////////////////////////////
									
								end++;
								FILE *fend = fopen("path_final.txt","wt");
								for(int i=0;i<path_final.size();i+=1){
										fprintf(fend,"%f %f %f\n",path_final[i].x,path_final[i].y,path_final[i].a);
								}
								fclose(fend);
							}
							if(cnt%10==0) fprintf(stderr,"No more scans!\n");
							usleep(1000*500);
							draw = 1;
					}
					cor_cnt++;		
			
			
			
			
				if(draw){
						setRGBmap(rgb2,&slamO, 200,255, 200);
						for(int i=0;i<path_final.size();i+=1){
								ocoord.x = path_final[i].x;ocoord.y = path_final[i].y;
								pix = slamO.toPixel(ocoord);
								setRGBpixBold(rgb2,pix.x,pix.y, Nx, 255,0, 0 );
								
								ocoord.x = odo[i].x;ocoord.y = odo[i].y;
								pix = slamO.toPixel(ocoord);
								setRGBpixBold(rgb2,pix.x,pix.y, Nx, 0,255, 0 );
						}
				}///IF DRAW
				
				
				if(cor_cnt>0 && draw) displayImage(0, rgb2,slamNy,slamNx);
				draw = 0;
						
				cnt++;
			}///While ! reset
			
			FILE *fmeas = fopen("pose_measurement.txt","wt");
			for(int i=0;i<pose_meas.size();i++){
					fprintf(fmeas,"%d %f %f %f %f %f %f\n",pose_meas_ind[i],pose_meas[i].x,pose_meas[i].y,pose_meas[i].a,
																															pose_meas_var[i].x,pose_meas_var[i].y,pose_meas_var[i].a);
			}
			fclose(fmeas);
			
			
			cor_threshold *= cor_th_step;
			
			fprintf(stderr,"Resetting!!!\n");
			reset = false;
			cnt = 0; cor_cnt = 0;
			float *map_data_ptr = O->getDataPtr();
			memcpy(map_data_ptr,orig_map_data,sizeof(float)*oNx*oNy);
			pose_updater.update(pose_meas, pose_meas_ind, pose_meas_var);
			for(int i=0; i<6;i++){
				fprintf(stderr,"Updating the odometry - iteration %d!\n",i);
				setRGBmap(rgb2,&slamO, 200,255, 200);
				
				mcl::pose odo_pose_calc(-20,-25,0);
				mcl::pose diffi;
				for(int j=0;j<odo.size()-1;j++){					
					ocoord.x = pose_updater.Xpos[3*j];ocoord.y =pose_updater.Xpos[3*j+1];
					pix = slamO.toPixel(ocoord);
					setRGBpixBold(rgb2,pix.x,pix.y, slamNx, 0,255, 0 );	
					
					diffi.set(pose_updater.odo[3*j],pose_updater.odo[3*j+1],pose_updater.odo[3*j+2]);
					odo_pose_calc = odo_pose_calc.integrateDifferential(diffi);
					ocoord.x = odo_pose_calc.x;ocoord.y =odo_pose_calc.y;
					pix = slamO.toPixel(ocoord);
					setRGBpixBold(rgb2,pix.x,pix.y, slamNx, 100,100, 100 );				
				}
				
				
				
				displayImage(0, rgb2,slamNy,slamNx);
				usleep(1000*1000);
				pose_updater.iterate();
			}
			pose_meas.clear();pose_meas_var.clear();pose_meas_ind.clear();
			
			for(int i=0;i<odo.size();i++){			
				odo[i].set(pose_updater.Xpos[3*i],pose_updater.Xpos[3*i+1],pose_updater.Xpos[3*i+2]);
			}
			path_final = odo;
			/**
			pose_meas = odo; ///< initial guess
			
			for(int i=0;i<pose_meas.size();i++){
				pose_meas_var.push_back(mcl::pose(100000,100000,100000));
				pose_meas_ind.push_back(i);
			}
			**/										 
			
			fprintf(stderr,"Updating the MAP distance transform\n");
			O->distanceTransform(dist, 0.8);
			
			fprintf(stderr,"Computing the correlation for whole data set\n");
			correlations.clear();
			for(int i=0;i<odo.size();i+=1){
				tmp_cor.v = computeCorrelation(scans[i], odo[i],8.0);
				tmp_cor.ind = i;
				correlations.push_back(tmp_cor);
				if(i%250==0) fprintf(stderr,"%.2f completed\n",100.0*(float)(i)/(float)odo.size()); 
			}
			quickSort(correlations);
		}
		return 0;
}